# -*- coding: utf-8 -*-
"""
Created on Mon Jan 10 16:39:27 2022

@author: Administrator
"""

import socket, threading, time, queue, os, re, sqlite3, logging
import numpy as np
IP = "192.168.140.1"
PORT = 14000
NAME = "C14AXX"
SEND_BUF_SIZE = 2*1024*1024
RECV_BUF_SIZE = 40*1024*1024
RECV_TIMEOUT = 10
START = b"<fxdz_socket_start>"
END = b"<fxdz_socket_end>"
FRM_S = b"<sub_frame_"
FRM_E = b"<sub_end_"
MAX_FRM_SIZE = 40*1024*1024
TAG_FAIL = -1
TAG_DONE = 1
TAG_ADD = 0
FRAME_RATE_WINDOWS = 20

blobsName = ["SCAN", "ADC", "RDC1", "RDC2", "RDC3", "RDC3P", "RDM", "SPM", "DPM", "APP", "RAIMG", "CLUTTER"]

def stamp2str(x): return time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime(x))

def splitRawBytes(data):
    SUB_FRAME=b"<sub_frame_"
    SUB_END=b"<sub_end_"
    b = data.split(SUB_FRAME)
    dataDict = {}
    if (len(b)<2): return dataDict
    for i in b:
        if len(i) <= 32: continue
        sp = i[:32].split(b">")
        if len(sp) < 2: continue
        name = sp[0]
        size = int.from_bytes(sp[1][:4], 'little')
        subS = 5 + len(name)
        subE = len(SUB_END) + len(name) + 1
        if (size + subS + subE == len(i)):
            subData = i[subS:-subE]
        else:
            subData = b"null"
        nameStr = str(name, encoding = "utf-8").upper()
        if nameStr == "HEAD": nameStr="ADdata" #old version compatible
        dataDict[nameStr] = subData
    return dataDict
    

class sqliteHyd():
    def __init__(self, path, radarNo=0):
        self.conn = sqlite3.connect(path + ".hyd")
        logging.info("open database successed!")
        self.c = self.conn.cursor()
        self.tableName = "Radar_%d"%int(radarNo)
        self.createHydTable()
        self.rowCounter = 0
    
    def createHydTable(self):
        self.c.execute('''DROP TABLE IF EXISTS %s'''%self.tableName)
        cmd = '''CREATE TABLE %s
            (RowNo INT PRIMARY KEY     NOT NULL,
                FrameNo          INT,
            Captime           REAL,
            Camtime           INT,
            Mark              INT,
            GPS               INT,\n'''%self.tableName
        for i in blobsName: cmd += "%s\tBLOB,\n"%i
        cmd = cmd[:-2]+ ");"
        self.c.execute(cmd)
        logging.info("table created")
        self.rowCounter = 0
        self.conn.commit()
    
    def DataInsert(self, FrameNo, Captime, rawBytes, Camtime):
        self.rowCounter += 1
        nameStr = ""
        valStr = ""
        if isinstance(rawBytes, dict):
            dataDict = rawBytes
        else:
            dataDict = splitRawBytes(rawBytes)
        for i in blobsName:
            nameStr += ", %s"%i
            valStr += ", ?"
        insert_query = "INSERT INTO %s (RowNo, FrameNo, Captime, Camtime, Mark, GPS%s) VALUES (?, ?, ?, ?, ?, ?%s)"%(self.tableName, nameStr, valStr)
        data_tuple = (self.rowCounter, FrameNo, Captime, Camtime, 0, 0)
        for i in blobsName:
            try:
                data_tuple += (dataDict[i],)
            except:
                data_tuple += (b"null",)
        self.c.execute(insert_query, data_tuple)
        self.conn.commit()
            
    def closeHyd(self):
        self.conn.close()

class CapRadarUdp(threading.Thread):
    def __init__(self, ip=IP, port=PORT, radarName=NAME, errFun=print):
        self.success = False
        self.ip = ip
        self.port = port
        self.radarName = radarName
        self.hydPath = None
        self.hydatabase = None
        threading.Thread.__init__(self)
        self.dataQueue = queue.LifoQueue(maxsize=3)
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        server_address = (ip, port)
        self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, RECV_BUF_SIZE)
        s_recv_buffer_size = self.sock.getsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF)
        print("socket receive buffer size is %d" % s_recv_buffer_size)
        # bind port
        print("starting connect to ip %s, port %s" % server_address)
        try:
            self.sock.bind(server_address)
        except Exception:
            errFun('[!] Server not found or not open or no network')
            return
        self.sock.setblocking(False)
        # self.setDaemon(True)
        self.frameCounter = 0
        self.cycletime = 0
        self.success = True
            
    def getFrameData(self):
        try:
            return self.dataQueue.get(block=False)
        except:
            return [None, None, None]
    
    def getFrameDataForce(self, timeout=10):
        start = time.time()
        while(1):
            try:
                frmData = self.dataQueue.get(block=False)
                if frmData is not None:
                    return frmData
            except:
                time.sleep(0.1)
            if time.time() - start > timeout:
                return [None, None, None]
    
    def _msgCheckTags(self, recvBytes):
        if len(self.blob) > MAX_FRM_SIZE:
            return TAG_FAIL
        if len(recvBytes) > 100:
            self.blob += recvBytes
            return TAG_ADD
        frm_s = re.match(b"%s.*>"%FRM_S, recvBytes)
        if frm_s:
            # print(f"{recvBytes}, {len(recvBytes)}")
            chklen = recvBytes[frm_s.end():]
            if len(chklen) != 4: return TAG_FAIL
            self.tag_length = np.frombuffer(chklen, 'I')[0]
            self.tag = recvBytes[frm_s.start()+len(FRM_S):frm_s.end()-1]
            self.blob = bytearray()
            return TAG_ADD
        frm_e = re.match(b"%s.*>"%FRM_E, recvBytes)
        if frm_e:
            # print(f"{recvBytes}, {len(recvBytes)}")
            tag = recvBytes[frm_e.start()+len(FRM_E):frm_e.end()-1]
            if tag == self.tag:
                if self.tag_length == len(self.blob):
                    return TAG_DONE
                else:
                    print(f"tag length {self.tag_length}, {len(self.blob)} not match")
                    return TAG_FAIL
            else:
                print(f"{tag}, {self.tag} not match")
                return TAG_FAIL
        self.blob += recvBytes

    def _checkDataBaseStatus(self):
        if self.hydatabase is None and self.hydPath is not None:
            self.hydatabase = sqliteHyd(self.hydPath)
        if self.hydatabase is not None and self.hydPath is None:
            self.hydatabase.closeHyd()
            self.hydatabase = None

    def run(self):
        self.stopFlag = 0
        self.blob = bytearray()
        self.tag = ''
        self.tag_length = 0
        lastRecvTime = time.time()
        findHeader = False
        recvTimeList = []
        while(1):
            if self.stopFlag == 1:
                return
            self._checkDataBaseStatus()
            try:
                receive_data, client = self.sock.recvfrom(2*1024*1024)
            except:
                time.sleep(0.05)
                if time.time()-lastRecvTime > RECV_TIMEOUT:
                    print("No recveive for %s s, please close if no need any more."%RECV_TIMEOUT)
                    self.sock.close()
                    self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
                    server_address = (self.ip, self.port)
                    self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, RECV_BUF_SIZE)
                    s_recv_buffer_size = self.sock.getsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF)
                    print("socket receive buffer size is %d" % s_recv_buffer_size)
                    # bind port
                    print("starting connect to ip %s, port %s" % server_address)
                    try:
                        self.sock.bind(server_address)
                    except Exception:
                        return
                    self.sock.setblocking(False)
                    self.setDaemon(True)
                    lastRecvTime = time.time()
                continue
            if findHeader == False:
                if len(receive_data) > 100: continue
                if receive_data[:len(START)] == START:
                    findHeader = True
                    tempData = {}
                continue
            else:
                if receive_data[:len(END)] != END:
                    if TAG_DONE == self._msgCheckTags(receive_data):
                        tempData[str(self.tag, encoding='utf-8')] = self.blob
                    continue
                else:
                    findHeader = False
                    frmData = tempData.copy()
                    lastRecvTime = time.time()
                    self.frameCounter += 1
                    if self.hydatabase is not None:
                        self.hydatabase.DataInsert(self.frameCounter, lastRecvTime, frmData, lastRecvTime)
                    # print("received %d frame at %.2f"%(self.frameCounter, lastRecvTime))
                    if len(recvTimeList) < FRAME_RATE_WINDOWS:
                        recvTimeList.append(lastRecvTime)
                    else:
                        self.cycletime = (recvTimeList[-1] - recvTimeList[0]) / (FRAME_RATE_WINDOWS - 1)
                        recvTimeList = []
                    if self.dataQueue.full():
                        self.dataQueue.queue.clear()
                    self.dataQueue.put([self.frameCounter, lastRecvTime, frmData])

    def stopRun(self):
        self.stopFlag = 1
        self.join(3)
        self.sock.close()

    def recoderStart(self, cd=""):
        if not os.path.exists(cd):
            os.makedirs(cd)
        if self.hydPath is not None:
            self.recoderClose()
        self.hydPath = f"{cd}/{stamp2str(time.time())}"

    def recoderClose(self):
        self.hydPath = None

    def getCurrCycleTime(self): return self.cycletime
    def isRecoding(self): return self.hydPath is not None

def CaputreFrame(acqNum = 1):
    udp = CapRadarUdp()
    udp.start()
    ret = []
    timeout = 10
    s = time.time()
    while(1):
        _,_,data = udp.getFrameData()
        if data is not None:
            ret.append(data)
            s = time.time()
            if len(ret) == acqNum: break
        else:
            time.sleep(0.5)
            if (time.time() - s) > timeout:
                logging.error("timeout")
                break
    udp.stopRun()
    return ret

#%% this cell will start receive radar data background     
if __name__ == "__main__":
    IP = "192.168.140.1"
    PORT = 14000
    NAME = b'0xC16A03'
    capRadar0 = CapRadarUdp(IP, PORT, NAME)
    capRadar0.start()
    # capRadar0.recoderStart("rawdata")
    #%% this cell is an example to get frame data from background 
    while(1):
        frmData = capRadar0.getFrameData()
        if(frmData[1] is not None):
            capRadar0.recoderClose()
            time.sleep(4)
            break
        time.sleep(0.1)
    """
    stopRun is very necessary when you want stop the capture
    """
    capRadar0.stopRun()

